#include <rclcpp/rclcpp.hpp>
#include <navigation/nav_core.hpp>
#include <tf2/LinearMath/Quaternion.h>

void demo(std::shared_ptr<NavCore> nav_core) {
    tf2::Quaternion q;
    q.setRPY(0, 0, -M_PI / 2);
    nav_core->state_manager->add_target(
        1.0, 1.0, 0.0,
        q.x(), q.y(), q.z(), q.w()
    );
    q.setRPY(0, 0, M_PI / 2);
    nav_core->state_manager->add_target(
        5.0, 0.0, 0.0,
        q.x(), q.y(), q.z(), q.w(), -1  // 添加到最后
    );
    nav_core->state_manager->add_target(
        7.0, 7.0, 0.0,
        q.x(), q.y(), q.z(), q.w(), -1  // 添加到最后
    );
    nav_core->state_manager->add_target(
        3.0, 2.0, 0.0,
        q.x(), q.y(), q.z(), q.w(), -1  // 添加到最后
    );
    q.setRPY(0, 0, 0);
    nav_core->state_manager->add_target(
        0.0, 0.0, 0.0,
        q.x(), q.y(), q.z(), q.w(), -1  // 添加到最后
    );
}

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto nav_core = std::make_shared<NavCore>();
    nav_core->init();
    // demo(nav_core);
    rclcpp::spin(nav_core);
    rclcpp::shutdown();
    nav_core->deviation_controller->pause();
    return 0;
}
